#include "crc.h"
#include "device_spi.h"
#include "gpio.h"
#include "math.h"
#include "modbus.h"
#include "speed_cntr.h"
#include "tim.h"
#include "user_device.h"
#include "user_global.h"
#include "usart.h"
#include "wwdg.h"
#include <setjmp.h>
#include <stdlib.h>
#include <string.h>

unsigned char server_address  = 0x11;

bit ready_active[KEY_START_COUNT] = {0};   // 传感器有效标志，只要检测到传感器发生沿变化，认为有效

bit pending_action = 0;                    // 新动作
uchar pending_role = 0;                    // 动作请求类别: 0 key1按键动作请求 1 key2按键动作请求; 2上位机动作
extern jmp_buf main_loop_env;

#define KEY_STOP_TIMER                   0xFFFFFFFF
#define SHAKE_LASTTIME                   1000               // 按键触发后，去抖确认时长us
#define KEY_PUSHED_CONFIRM_LASTTIME      20000              // 按键触发后，确认持续时长us
#define KEY_PUSHED_LONG_CONFIRM_LASTTIME 8000000            // 按键触发后，长按确认持续时长us
#define KEY_EVENT_COUNT                 KEY_START_COUNT
ulong key_stamp[KEY_EVENT_COUNT] = { KEY_STOP_TIMER,KEY_STOP_TIMER,KEY_STOP_TIMER,KEY_STOP_TIMER};
ulong shake_stamp[KEY_EVENT_COUNT] = { KEY_STOP_TIMER,KEY_STOP_TIMER,KEY_STOP_TIMER,KEY_STOP_TIMER};
ulong key_longtime_stamp[3] = {KEY_STOP_TIMER};
uchar event[KEY_EVENT_COUNT];                                         // 左右缓冲输入事件
ushort auto_key_pending[KEY_EVENT_COUNT] = {0};                       // 按键触发事件申请的动作计数

#define MAX_RETRY_TIMES     1
uchar key_pushed_count = 0;                                           // 按键触发计数值，所有申请动作空闲时，清零
uchar me_state[KEY_START_COUNT] = {ME_STATE_IDLE,ME_STATE_IDLE,ME_STATE_IDLE,ME_STATE_IDLE};
uchar me_retry[KEY_START_COUNT] = {0};
uchar me_lastEvent[KEY_START_COUNT] = {0};        // 最后一次记录的异常事件
bit high_priority_me = 0;
uchar me_max_retry_times[KEY_START_COUNT] = {MAX_RETRY_TIMES,MAX_RETRY_TIMES,MAX_RETRY_TIMES};

uchar me_key_pushed_count[KEY_START_COUNT] = {0};

ushort start_pressed_duration[KEY_START_COUNT];
bit ready_for_work[KEY_START_COUNT] = {0};

bit enter_configure_work[KEY_START_COUNT] = {0};

void Set_Starts_Led(uchar n,uchar model);

enum LED_MODE{
    LED_ON_CONST,           // 常亮
    LED_OFF_CONST,          // 常灭
    LED_BLINK1,             // 闪烁
    LED_BLINK2,
    LED_BLINK3,
    LED_BLINK_AUTOMATIC,    // 闪烁
};

LED_CNTRL led_mode_pool[6] = {
    {0xFFFF,    1,      0},
    {0xFFFF,    0,      1},
    {0xFFFF,    1000,   1000},
    {0xFFFF,    200,    200},
    {0xFFFF,    2000,   2000},
    {0xFFFF,    100,    100},
};

LED_CNTRL led_cnt[KEY_START_COUNT];

#define MAX_FIX_COUNT     20
FIX_VALUE fix;

//////////////////////////////////////////////////////////////
void Update_IO(void)
{
    ro_bit.s1_at_home  = (IO_Read(IO_S1_HOME) && (!srd[S1].running) && (!ready_for_work[S1]));
    ro_bit.s2_at_home  = (IO_Read(IO_S2_HOME) && (!srd[S2].running) && (!ready_for_work[S2]));
    ro_bit.s3_at_home  = (IO_Read(IO_S3_HOME) && (!srd[S3].running) && (!ready_for_work[S3]));
    ro_bit.s4_at_home  = IO_Read(IO_S4_HOME);
    ro_bit.s1_at_limit = IO_Read(IO_S1_LIMIT);
    ro_bit.s2_at_limit = IO_Read(IO_S2_LIMIT);
    ro_bit.s3_at_limit = IO_Read(IO_S3_LIMIT);
    ro_bit.s4_at_limit = IO_Read(IO_S4_LIMIT);
}

///////////////////////////////////////////////////////////////
// start event
long continue_press_debug = 0;

// 检测到按键按下返回1,否则返回0
bit Key_Pushed(uchar key)
{

    volatile bit pressed;
    bit key_long_pressed;

    assert(key < KEY_START_COUNT)

    pressed = IO_Read(key + IO_START_S1_PRESSED);

    // 检测按键状态变化
    if(pressed != Test_Bit(RO_BIT_FIRST, KEY_S1_PRESSED + key))
    {
        Set_Bit(RO_BIT_FIRST, KEY_S1_PRESSED  + key, pressed);
        if(pressed)
        {
            if(key_stamp[key] == TIMER_STOPED)
            {
                TIMER_START(key_stamp[key]);
            }
            TIMER_START(shake_stamp[key]);
        }
         else {
        }
    }

    if(!pressed)
    {
        if (key_stamp[key] != TIMER_STOPED && SystemTick() - key_stamp[key] > KEY_PUSHED_LONG_CONFIRM_LASTTIME)
        {
            TIMER_STOP(key_stamp[key]);
            enter_configure_work[key] = 1;
            continue_press_debug = 0;
            Set_Starts_Led(key,LED_BLINK_AUTOMATIC);
            return 0;
        }
        else if(key_stamp[key] != TIMER_STOPED && SystemTick() - key_stamp[key] > KEY_PUSHED_CONFIRM_LASTTIME)
        {
            TIMER_STOP(key_longtime_stamp[key]);
            TIMER_STOP(key_stamp[key]);
            continue_press_debug = 0;
            return 1;
        }

        if(shake_stamp[key] != TIMER_STOPED && SystemTick() - shake_stamp[key] > SHAKE_LASTTIME)
        {
            TIMER_STOP(key_stamp[key]);
            TIMER_STOP(shake_stamp[key]);
        }
        return 0;
    }
    else
    {
        continue_press_debug++;
        if(key_stamp[key] != TIMER_STOPED)   {
            start_pressed_duration[key] = (SystemTick() - key_stamp[key]) / 1000;
        }
    }

    return 0;
}

// 软开关指示灯控制函数,周期 == 亮-->灭 | 亮-->灭 |......
void Key_Led_Ctrl(uchar n)
{
#define LED_BLINK_FORERVER            0xFFFF        // led 始终按设定模式执行

#define STATE_READY_TO_LIGHT_OFF      0             // 准备置灭LED
#define STATE_READY_TO_LIGHT_ON       1             // 准备点亮LED

    static uchar led_state[KEY_START_COUNT] = {0};
    static ulong led_on_time[KEY_START_COUNT] = {0};
    static ulong led_off_time[KEY_START_COUNT] = {0};
    static ushort led_on_duration[KEY_START_COUNT] = {0};

    LED_CNTRL *led = &led_cnt[n];
    if(led_on_duration[n]!= led->led_on_duration)
    {
        if(!led_on_duration[n])
        {
            IO_Set(IO_START_S1_LED + n);
            led_state[n] = STATE_READY_TO_LIGHT_OFF;
            led_on_time[n] = SystemTick();
        }
        led_on_duration[n] = led->led_on_duration;
    }

    if(led->led_blink_times)
    {
        switch (led_state[n]) {
        case STATE_READY_TO_LIGHT_OFF:
            if(led->led_off_duration)
            {
                // 亮时间超时，准备置灭
                if((SystemTick() - led_on_time[n]) > (ulong)led->led_on_duration * 1000 )
                {
                    IO_Reset(IO_START_S1_LED + n);
                    led_state[n] = STATE_READY_TO_LIGHT_ON;
                    led_off_time[n] = SystemTick();
                }
            }
            else {
                led_state[n] = STATE_READY_TO_LIGHT_ON;
            }
            break;
        case STATE_READY_TO_LIGHT_ON:
            if(led->led_on_duration)
            {
                // 灭时间超时，准备点亮
                if( (SystemTick() - led_off_time[n]) > (ulong)led->led_off_duration * 1000 )
                {
                    IO_Set(IO_START_S1_LED + n);
                    led_state[n] = STATE_READY_TO_LIGHT_OFF;
                    led_on_time[n] = SystemTick();
                    if(led->led_blink_times != LED_BLINK_FORERVER)
                        --led->led_blink_times;
                }
            }
            else {
                led_state[n] = STATE_READY_TO_LIGHT_OFF;
            }
        default:
            break;
        }
    }
}

void Set_Starts_Led(uchar n,uchar model)
{
    memcpy(&led_cnt[n],&led_mode_pool[model],sizeof(LED_CNTRL));
}

bit All_Starters_Idle()
{
    uchar i;
    for(i = 0; i < KEY_START_COUNT; ++i)
    {
        if(me_state[i] != ME_STATE_IDLE)
            return 0;
    }
    return 1;
}

bit Other_Starters_Idle(uchar starter)
{
    uchar i;
    for(i = 0; i < KEY_START_COUNT; ++i)
    {
        if(i == starter)
            continue;

        if(me_state[i] != ME_STATE_IDLE)
            return 0;
    }
    return 1;
}

// 返回所有非空闲的starter中，如果都处于wait状态返回1，否则返回0
bit All_Starters_Wait()
{
    uchar i;
    uchar has_wait = 0;
    for(i = 0; i < KEY_START_COUNT; ++i)
    {
        if(me_state[i] == ME_STATE_WAIT)
            has_wait = 1;
        if(me_state[i] == ME_STATE_IDLE)
            continue;
        if(me_state[i] != ME_STATE_WAIT)
            return 0;
    }
    return has_wait;
}

// 返回等待的starer中优先级较高的starter，未找到返回0xff
uchar High_Priority_Waiter()
{
    uchar i;
    uchar high_wait = 0xFF;
    uchar min_count = 0xFF;
    for(i = 0; i < KEY_START_COUNT; ++i)
    {
        if(me_state[i] == ME_STATE_WAIT)
        {
            if(me_key_pushed_count[i] <= min_count)
            {
                min_count = me_key_pushed_count[i];
                high_wait = i;
            }
        }
    }
    return high_wait;
}

void Clear_Event(uchar index)
{
    event[index] = EVENT_NONE;
}

void Post_Event(uchar index, uchar etype)
{
    if(index < KEY_EVENT_COUNT)
        event[index] = etype;
}

uchar Take_Event(uchar index)
{
    uchar e;
    if(index >= KEY_EVENT_COUNT)
        return 0;
    e = event[index];
    event[index] = 0;
    return e;
}

void Me_Init(uchar index)
{
    auto_key_pending[index] = 0;
    me_retry[index] = 0;
    Clear_Event(index);
    me_lastEvent[index] = EVENT_NONE;
}

// 按钮开关按键触发处理
void Process_Starter(uchar index)
{
    volatile bit key_pushed = Key_Pushed(index);

    if(!key_pushed)
        return;

    ++key_pushed_count;
    if(All_Starters_Idle())
        key_pushed_count = 0;

    switch(me_state[index])
    {
    case ME_STATE_IDLE:
        ready_for_work[index] = 0;
        me_state[index] = ME_STATE_WAIT;
        Set_Starts_Led(index,LED_BLINK1);
        me_key_pushed_count[index] = key_pushed_count;
        break;
    case ME_STATE_WAIT:
        ready_for_work[index] = 1;
        me_state[index] = ME_STATE_IDLE;
        Set_Starts_Led(index,LED_OFF_CONST);
        // ready_for_work[index] = 0;
        break;
    case ME_STATE_WORKING:
        if(IO_Read(IO_S1_HOME+index) && srd_desc[index].srd->running)
            break;
        me_state[index] = ME_STATE_IDLE;
        Set_Starts_Led(index,LED_OFF_CONST);
        if(rw_word.action == (OPEN_S1_DRAWER + (index << 1)) || rw_word.action == (CLOSE_S1_DRAWER + (index << 1)))
        {
            IO_Set(IO_V1);
            IO_Set(IO_V2);
            IO_Set(IO_V3);
            IO_Set(IO_V4);

            // 打开抽屉时正在预备柜门动作
            if(index < S4 && srd_desc[S4].srd->running)
            {
                srd_desc[S4].srd->max_index = 0;
                srd_desc[S4].srd->speed_index = 0;
                srd_desc[S4].srd->step_left = srd_desc[S4].srd->stop_step;
                srd_desc[S4].srd->target = srd_desc[S4].srd->step_left;
            }
            if(srd_desc[index].srd->running)
            {
                srd_desc[index].srd->max_index = 0;
                srd_desc[index].srd->speed_index = 0;
                srd_desc[index].srd->step_left = srd_desc[index].srd->stop_step;
                srd_desc[index].srd->target = srd_desc[index].srd->step_left;
            }
        }
        ready_for_work[index] = 0;
        break;
    }
}

bit Can_Lock_Action(bit ignore_error)
{
    if(ps_server_busy)
        return 0;
    if(ro_word.server_status != STATUS_IDLE && !ignore_error)
        return 0;
    return 1;
}

void Process_Event(uchar index)
{
    uchar high_waiter = 0;
    bit ignore_error = 0;
    static ulong me_stamp[KEY_START_COUNT] = {TIMER_STOPED, TIMER_STOPED, TIMER_STOPED, TIMER_STOPED};
    assert(index < KEY_START_COUNT);

    switch(me_state[index])
    {
    case ME_STATE_IDLE:
        ready_for_work[index] = 0;
        me_stamp[index] = TIMER_STOPED;
        if (index == S4)
            return;
        if (rw_word.action == CLOSE_S4_DRAWER)
            return;
        if (srd_desc[index].srd->step_left > srd_desc[index].srd->stop_step && IO_Read(IO_S1_HOME + index))
            srd_desc[index].srd->step_left = 0;
        break;
    case ME_STATE_WAIT:
        if (!Other_Starters_Idle(index))
        {
            if (All_Starters_Wait())
            {
                high_waiter = High_Priority_Waiter();
                // 所有starter都在等待，且当前等待优先级最高, 或者未找到最高优先级,启动当前starter
                if(high_waiter != 0xff && high_waiter != index)
                    return;
            }
            else
                return;
        }
        ready_for_work[index] = 1;
        // 禁止按钮触发，返回，等待
        if(Test_Bit(RW_BIT_FIRST, index))
        {
            ready_for_work[index] = 0;
            return;
        }
        ignore_error = (me_retry[index] < MAX_RETRY_TIMES);
        if (Can_Lock_Action(ignore_error) && index < KEY_STOP)
        {
            if (me_stamp[index] == TIMER_STOPED )
                TIMER_START(me_stamp[index]);
            if ((me_stamp[index] != TIMER_STOPED && TIMER_OUT(me_stamp[index],1000000)))
            {
                if (enter_configure_work[index])
                {
                    TIMER_STOP(me_stamp[index]);
                    rw_word.action = CONFIG_S1_PARAM+index;
                    pending_action = 1;
                    //pending_role = ACTION_PENDING_KEY1_EVENT + index;
                    me_state[index] = ME_STATE_WORKING;
                }
                else
                {
                    if (IO_Read(IO_S1_HOME + index))
                        rw_word.action = OPEN_S1_DRAWER + (index << 1);
                    else
                        rw_word.action = CLOSE_S1_DRAWER + (index << 1);
                    pending_action = 1;
                    pending_role = ACTION_PENDING_KEY1_EVENT + index;
                    me_state[index] = ME_STATE_WORKING;
                    Set_Starts_Led(index,LED_ON_CONST);
                }
            }
        }

        break;
    case ME_STATE_WORKING:
        if(ps_server_busy)
            return;
        ready_for_work[index] = 0;
        me_state[index] = ME_STATE_IDLE;
        if(ro_word.server_status != STATUS_IDLE)
            Set_Starts_Led(index,LED_BLINK2);
        else
            Set_Starts_Led(index,LED_OFF_CONST);
        break;
    }
}

bit Fix_Level(uchar IO)
{
    static uchar fix_filter = 0xff;
    static uchar fix_sum = 8;
    bit state = IO_Read(IO);
    fix_sum -= ((fix_filter & 0x80) >> 7);
    fix_filter = (fix_filter << 1) | state;
    fix_sum += state;
    return (fix_sum > 4);
}

void Reset_Fix()
{
    fix.index = 0;
    fix.exist = 0;
    fix.state = Fix_Level(0);
    fix.enable = 1;
}

void Update_fix()
{
    bit fix_level;

    if (!fix.enable)
        return;

    fix_level = Fix_Level(0);
    if (fix.state != fix_level)
    {
        fix.state = fix_level;
        if (fix.state)
        {
            if (IO_Read(0))
                fix.exist |= (1 << fix.index);
            else
                fix.exist &= ~(1 << fix.index);
            fix.exist &= 0x1F;
            ++fix.index;
        }
    }
}

void Lock_V(uchar index)
{
    if(index > KEY_START_COUNT)
        return;
    IO_Set(IO_V1+index);
}

void Release_V(uchar index)
{
    if(index > KEY_START_COUNT)
        return;
    IO_Reset(IO_V1+index);
}


void Online_Monitor_V(uchar index)
{
    /*if(ps_server_busy)
        return;
    if((v_stamp[index] != TIMER_STOPED && TIMER_OUT(v_stamp[index],60000000)))
    {
        TIMER_STOP(v_stamp[index]);
        Release_V(index);
    }*/
}

void Param_Save(uchar role)
{
    if (rw_word.s_home_step_flag[role])
        rw_word.s_home_step_buffer[role]++;
}

////////////////////////////////////////////////////////////////////////////////
// machine base

void Step(uchar role, bit level)
{
    //IO_Z_STEP(level);
}

void Start_Timer(uchar role)
{
    timer_enable(srd_desc[role].timer);
}

void Set_Timer(uchar role)
{
    ushort micro = srd_desc[role].srd->micro;
    ushort tick = FREQ_TABLE(srd_desc[role].srd->speed_index);
    if(micro)
    {
        // 默认使用16细分，如果使用了其他细分,改变pwm的频率
        tick = tick * FACILITYMICRO / (micro > FACILITYMICRO ? FACILITYMICRO : micro);
        tick = tick > MAX_C ? MAX_C : tick;
    }
    timer_autoreload_value_config(srd_desc[role].timer, tick*2);
    timer_channel_output_pulse_value_config(srd_desc[role].timer, srd_desc[role].timer_ch, tick);
}

void Stop_Timer(uchar role)
{
    timer_disable(srd_desc[role].timer);
}

// 启动电机
void Start_Motor(uchar role)
{
    SpeedRampData *srd = srd_desc[S1+role].srd;
    long *distance = (long*)&ro_word.mi[S1+role].distance;
    *distance = 0;
    if(srd->dir)
        IO_Set(IO_S1_CW + role);
    else
        IO_Reset(IO_S1_CW + role);
    Set_Timer(srd->role);
    Start_Timer(srd->role);
    IO_Set(IO_S1_EN + role);
    IO_Reset(IO_S1_LOCK + role);
    Drv_Work_Full(role);
}

// 停止电机
void Stop_Motor(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;
    long  *distance = (long*)&ro_word.mi[role].distance;
    ushort *duration = (ushort*)&ro_word.mi[role].duration;
    if (srd->dir == CCW)
        *distance = -srd->step_count;
    else
        *distance = srd->step_count;
    *duration = (ushort)((SystemTick() - srd->start_time)/US_TO_TICK(1000));
    srd->need_cleanup = 0;
    srd->running = 0;
    IO_Set(IO_S1_LOCK + role);
    IO_Reset(IO_S1_EN + role);  // debug
    if(role == S4)
    {
        Drv_Work_Lock(S4);
        Delay_Tick_Loop(240000);
        Drv_Work_Full(S4);
    }
    else
        Drv_Disable(role);
    //Lock_V(role);
    //Drv_Work_Full(role);
}

void Init_Move(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;
    uchar home = srd_desc[role].home;
    uchar limit = srd_desc[role].limit;
    uchar limit2 = srd_desc[role].limit2;
    uchar limit3 = srd_desc[role].limit3;

    srd->init = Start_Motor;
    srd->step = Step;
    srd->start_timer = Set_Timer;
    srd->stop_timer = Stop_Timer;
    srd->cleanup = Stop_Motor;
    srd->home_state = IO_Read(home);
    srd->limit_state = IO_Read(limit);
    srd->limit_state2 = IO_Read(limit2);
    srd->limit_state3 = IO_Read(limit3);
    srd->hit_home = 0;
    srd->hit_limit = 0;
    srd->hit_limit2 = 0;
    srd->hit_limit3 = 0;

    srd->first_stage_speed = SPEED2INDEX(FIRST_STAGE_SPEED);
    srd->first_stage_step = DISTANCE2STEP(FIRST_STAGE_DISTANCE);

    srd->second_stage_speed = SPEED2INDEX(rw_word.second_speed[role]); //SPEED2INDEX(SECOND_STAGE_SPEED);
    srd->second_stage_step = rw_word.second_step[role];   //DISTANCE2STEP(SECOND_STAGE_DISTANCE);
}

// 执行移动指令时, 每一步调用一次
void Move_To_Update(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;
    uchar home = srd_desc[role].home;
    uchar limit = srd_desc[role].limit;
    uchar limit2 = srd_desc[role].limit2;
    uchar limit3 = srd_desc[role].limit3;

    Update_IO();
    Param_Save(role);

    if(IO_Read(home) != srd->home_state)
    {
        srd->home_state = !srd->home_state;
        srd->hit_home = 1;
        ro_word.pos[role] = 0;
        srd->stamp = SystemTick();
        srd->step_left = labs(srd->target);
    }
    else if(limit && (IO_Read(limit) != srd->limit_state))
    {
         srd->limit_state = !srd->limit_state;
         srd->hit_limit  = 1;
         if(srd->limit_state && srd->step_left > srd->limit_step)
         {
            srd->step_left = srd->limit_step;
            srd->reset_monitor_step = 0;
         }
    }
    else if(limit2 && (IO_Read(limit2) != srd->limit_state2))
    {
         srd->limit_state2 = !srd->limit_state2;
         srd->hit_limit2 = 1;
         if(srd->limit_state2 && srd->step_left > srd->limit_step)
            srd->step_left = srd->limit_step;
    }
    else if(limit3 && (IO_Read(limit3) != srd->limit_state3))
    {
         srd->limit_state3 = !srd->limit_state3;
         srd->hit_limit3  = 1;
         if(srd->limit_state3 && srd->step_left > srd->limit_step)
            srd->step_left = srd->limit_step;
    }
    else
        ro_word.pos[role] += srd->step_offset;
}

void T1_Move_To_Update(uchar role)
{
    Move_To_Update(role);
//    Update_fix();
}

// 执行移动指令时, 每一步调用一次
void Reset_To_Update(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;
    uchar home = srd_desc[role].home;
    uchar limit = srd_desc[role].limit;

    Update_IO();

    if(IO_Read(home) != srd->home_state)
    {
        srd->home_state = !srd->home_state;
        srd->hit_home = 1;
        ro_word.pos[role] = 0;
        srd->stamp = SystemTick();
        srd->step_left = labs(srd->target);
    }
    if(IO_Read(limit) != srd->limit_state)
    {
         srd->limit_state = !srd->limit_state;
         srd->hit_limit  = 1;
    }
    else
        ro_word.pos[role] += srd->step_offset;
}

void Do_Action_Stop(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;

    if (srd->running)
    {
        Stop_Timer(role);
        Stop_Motor(role);
    }
}

bit Do_Action_Reset(uchar role, ushort reset_speed)
{
    SpeedRampData *srd = srd_desc[role].srd;

    Release_V(role);
//    Delay_Tick_Loop(300000);

    Init_Move(role);
    srd->update = Reset_To_Update;
    srd->target = srd->home_state ? srd->reset_step : -srd->reset_step;
    speed_cntr_Move(srd,
                    srd->home_state ? (rw_word.mp[role].max_distance<<1) : -(rw_word.mp[role].max_distance<<1),
                    reset_speed);

    while(srd->running)
    {
        // if(srd->hit_limit || ro_word.pos[role] < srd->v_lock_pos)
        //     Release_V(role);
        if(srd->step_count > srd->v_lock_pos)
            Lock_V(role);
        Process_Main_Loop();
    }

    // 检测到新事件
    if(me_state[role] == ME_STATE_IDLE || me_state[role] == ME_STATE_WAIT)
    {
        Reset_RO_Bit(S1_HOME_SENSOR_ERROR + role );
        Lock_V(role);
        return 1;
    }

    if(role != S4)
        Delay_Tick_Loop(1000000);

    Lock_V(role);


    if(srd_desc[role].home && !IO_Read(srd_desc[role].home))
    {
        Set_RO_Bit(S1_HOME_SENSOR_ERROR + role );
        ro_word.server_status = S1_STATUS_HOME_SENSOR_ERROR + role;
        return 0;
    }
    else
        Reset_RO_Bit(S1_HOME_SENSOR_ERROR + role );

    // 复位完成，移动到负坐标位置
    Do_Action_Move_To(role,-srd->reset_step,rw_word.mp[role].max_speed);
    return 1;
}

void Do_Action_Reset_NoWait(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;

    Init_Move(role);
    srd->update = Reset_To_Update;
    srd->target = srd->home_state ? srd->reset_step : -srd->reset_step;
    speed_cntr_Move(srd,
                    srd->home_state ? rw_word.mp[role].max_distance : -rw_word.mp[role].max_distance,
                    rw_word.mp[role].max_speed);
}

void Do_Action_Move_To(uchar role, long target, ushort speed)
{
    SpeedRampData *srd = srd_desc[role].srd;

    Init_Move(role);
    srd->update = Move_To_Update;
    srd->target = target;

    speed_cntr_Move(srd,
                    target - ro_word.pos[role],
                    speed);

    while(srd->running)
        Process_Main_Loop();
}

// machine base end
///////////////////////////////////////////////////////////////

void Clear_Limit(uchar role)
{
    srd_desc[role].limit2 = NULL_IN;
    srd_desc[role].limit3 = NULL_IN;
}

void Init_S_Move(uchar role)
{
    Init_Move(role);
}

//停机，停止所有在运行的电机
void Do_Action_S_Stop(uchar role)
{
    Do_Action_Stop(role);
}

void Do_Action_S_Move_To(uchar role, long target, ushort speed)
{
    Do_Action_Move_To(role, target, speed);
}

// S1/S2/S3/S4 关闭
uchar Do_Action_V_Close_Drawer(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;
    uchar home = srd_desc[role].home;

    if(IO_Read(home))
    {
        // 在复位位置，且坐标不正确，清除坐标，
        // 因为检测到复位光耦遮挡，且坐标为关闭坐标值，发送打开/关闭将会不动作
        if(ro_word.pos[role] > srd->reset_step*2)
            ro_word.pos[role] = 0;
        return 1;
    }

    return Do_Action_Reset(role,rw_word.mp[role].max_speed);
}

// S1/S2/S3/S4 打开
bit Do_Action_V_Open_Drawer(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;
    uchar home = srd_desc[role].home;

    assert(role >= S1 && role <= S4);

    Release_V(role);
    Delay_Tick_Loop(100000);
    if(IO_Read(home))
    {
        // 在复位位置，且坐标不正确，清除坐标，
        // 因为检测到复位光耦遮挡，且坐标为关闭坐标值，发送打开/关闭将会不动作
        if(ro_word.pos[role] > srd->reset_step*2)
            ro_word.pos[role] = 0;
    }

    if(IO_Read(srd_desc[role].limit) && ro_word.pos[role] > srd->reset_step*2)
    {
        Reset_RO_Bit(S1_MOTOR_ERROR + role - S1);
        Reset_RO_Bit(S1_STATUS_LIMIT_SENSOR_ERROR + role - S1);
        return 1;
    }

    Init_Move(role);
    srd->update = Move_To_Update;
    srd->target = rw_word.mp[role].max_distance;

    speed_cntr_Move(srd,
                    srd->target,
                    rw_word.mp[role].max_speed);

    while(srd->running)
    {
        if(srd->step_count > srd->v_lock_pos)
            Lock_V(role);
        Process_Main_Loop();
    }

    Lock_V(role);
    // 打开动作后还在复位(关闭)位置
    if(IO_Read(home))
    {
        ro_word.server_status = S1_STATUS_MOTOR_ERROR + role - S1;
        Set_RO_Bit(S1_MOTOR_ERROR + role - S1);
        return 0;
    }
    else
        Reset_RO_Bit(S1_MOTOR_ERROR + role - S1);

    if(!srd->hit_limit)
    {
        //ro_word.server_status = S1_STATUS_LIMIT_SENSOR_ERROR + role - S1;
        Set_RO_Bit(S1_STATUS_LIMIT_SENSOR_ERROR + role - S1);
        return 0;
    }
    else
        Reset_RO_Bit(S1_STATUS_LIMIT_SENSOR_ERROR + role - S1);

    return 1;
}

void S4_reset_monitor(void)
{
    uint8_t role = S4;
    SpeedRampData *srd = srd_desc[role].srd;
    uint8_t limit = srd_desc[role].limit;
    assert(role >= S1 && role <= S4);

    if (!limit)
        return;

    if (!IO_Read(limit))
        return;

    if (S4_LIMIT_BUFFER_STEP < srd->reset_monitor_step)
        return;
    srd->update = Move_To_Update;
    srd->target = S4_LIMIT_BUFFER_STEP - srd->reset_monitor_step;

    speed_cntr_Move(srd,
                    srd->target,
                    rw_word.mp[role].max_speed);

    while(srd->running)
        Process_Main_Loop();
}
//////////////////////////////////////////////////////////////////////////////////

void Do_Action_StandBy()
{
    uchar i = 0;
    for(i = 0; i < MACHINE_COUNT; ++i)
        Do_Action_Stop(i);
}

void Do_Action_Home()
{
    uchar i = 0;

    Do_Action_Reset_NoWait(S1);
    Do_Action_Reset_NoWait(S2);
    Do_Action_Reset_NoWait(S3);
//    Do_Action_Reset_NoWait(S4);
    while(srd_desc[S1].srd->running ||srd_desc[S2].srd->running || \
          srd_desc[S3].srd->running )
    {
        Process_Main_Loop();
    }
    for(i = 0; i < 3; ++i)
    {
        if(!srd_desc[S1 + i].srd->hit_home)
        {
            Write_RO_bit(S1_HOME_SENSOR_ERROR+i,1,1);
            ro_word.server_status = S1_STATUS_HOME_SENSOR_ERROR + i;
        }
        else
            Write_RO_bit(S1_HOME_SENSOR_ERROR+i,0,1);
    }

    // 复位完成后关闭闸门
    Do_Action_V_Close_Drawer(S1);
    Do_Action_V_Close_Drawer(S2);
    Do_Action_V_Close_Drawer(S3);

    // 复位完成，移动到负坐标位置
    Do_Action_Move_To(S1,-srd_desc[S1].srd->reset_step,rw_word.mp[S1].max_speed);
    Do_Action_Move_To(S2,-srd_desc[S1].srd->reset_step,rw_word.mp[S2].max_speed);
}

void Do_Action_Selftest(void)
{
    Update_IO();

    Do_Action_Home();
}

void Do_Action_Disable()
{
    uchar i = 0;
    for(i = 0; i < MACHINE_COUNT; ++i)
        IO_Reset(IO_S1_EN + i);
}

void Do_Action_home_step_buffer(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;
    uchar home = srd_desc[role].home;

    rw_word.s_home_step_flag[role] = 1;
    rw_word.s_home_step_buffer[role] = 0;

    Release_V(role);
    Delay_Tick_Loop(100000);

    Init_Move(role);
    srd->update = Move_To_Update;
    srd->target = rw_word.mp[role].max_distance;

    speed_cntr_Move(srd,
                    srd->target,
                    rw_word.mp[role].max_speed);

    while(srd->running)
    {
        Process_Main_Loop();
        if(!IO_Read(home))
        {
            srd->step_left = 0;
            me_state[role] = ME_STATE_IDLE;
            srd_desc[role].srd->run_state = 0;
            srd_desc[role].srd->need_cleanup = 1;
            break;
        }
    }
    Lock_V(role);

}

void Config_Param(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;
    uchar home = srd_desc[role].home;

    assert(role >= S1 && role <= S4);
    // 判断S1抽屉目前是关闭还是打开状态
    if(IO_Read(home))
    {
        rw_word.s_home_step_flag[role] = 1;
        rw_word.s_home_step_buffer[role] = 0;

        Release_V(role);
        Delay_Tick_Loop(100000);

        Init_Move(role);
        srd->update = Move_To_Update;
        srd->target = rw_word.mp[role].max_distance;

        speed_cntr_Move(srd,
                        srd->target,
                        rw_word.mp[role].max_speed>>5);

        while(srd->running)
        {
            Process_Main_Loop();
            if(!IO_Read(home))
            {
                srd->step_left = 100;
                me_state[role] = ME_STATE_IDLE;
                srd_desc[role].srd->run_state = 0;
                srd_desc[role].srd->need_cleanup = 1;
                break;
            }
        }
        Lock_V(role);
        // 更新当前flash中的参数
        rw_word.s_home_step_buffer[role] += 40;
        if (rw_word.s_home_step_buffer[role] > srd[role].reset_step)
            dev_run_param.s_home_step_buffer[role] = rw_word.s_home_step_buffer[role];
    }

    if (rw_word.s_home_step_buffer[role] > 0)
    {
        // 更新当前flash中的参数
        Write_Dev_Param();
    }
    rw_bit.key_long_pressed_s1 = 0;
    rw_bit.key_long_pressed_s2 = 0;
    rw_bit.key_long_pressed_s3 = 0;
}


////////////////////////////////////////////////////////////////////////////////
/// \brief Process_Action
///
void Process_Action()
{
    uchar role;
    uint32_t index = 0;
    switch (rw_word.action)
    {
    case STANDBY:
        Do_Action_StandBy();
        ps_server_busy = 0;
        break;
    case MRESET:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        Do_Action_Home();
        ps_server_busy = 0;
        break;
    case OPEN_S1_DRAWER:
    case OPEN_S2_DRAWER:
    case OPEN_S3_DRAWER:
    case OPEN_S4_DRAWER:
        index = S1+((rw_word.action-OPEN_S1_DRAWER)>>1);
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;

        if(rw_word.action != OPEN_S4_DRAWER && !IO_Read(IO_S4_LIMIT))
        {
            if(IO_Read(IO_S4_HOME))
                Do_Action_V_Open_Drawer(S4);
            else
            {
                Do_Action_V_Open_Drawer(S4);
            }
        }

#if PCB_VERSION == PCB_A1
        //srd[S4].reset_monitor_step
        if (index != S4
            && (srd_desc[3].srd->reset_monitor_step - 1) != S4_LIMIT_BUFFER_STEP)
        {
            S4_reset_monitor();
        }
#endif

        if(me_state[index] == ME_STATE_WORKING)
        {
            if (!Test_Bit(RW_BIT_FIRST, index))
                Do_Action_V_Open_Drawer(index);
            else
            {
                me_state[index] = ME_STATE_WAIT;
                Set_Starts_Led(index, LED_BLINK1);
            }
        }
        ps_server_busy = 0;
        break;
    case CLOSE_S1_DRAWER:
    case CLOSE_S2_DRAWER:
    case CLOSE_S3_DRAWER:
    case CLOSE_S4_DRAWER:
        ps_server_busy = 1;
        index = S1+((rw_word.action-CLOSE_S1_DRAWER)>>1);
        ro_word.server_status = STATUS_IDLE;
        if(rw_word.action != CLOSE_S4_DRAWER && !IO_Read(IO_S4_LIMIT))
        {
            if(IO_Read(IO_S4_HOME))
                Do_Action_V_Open_Drawer(S4);
            else
            {
                // Do_Action_V_Close_Drawer(S4);
                Do_Action_V_Open_Drawer(S4);
            }
        }
        if(rw_word.action == CLOSE_S4_DRAWER )
        {
            Do_Action_V_Close_Drawer(S1);
            Do_Action_V_Close_Drawer(S2);
            Do_Action_V_Close_Drawer(S3);
        }

        if(me_state[index] == ME_STATE_WORKING)
            Do_Action_V_Close_Drawer(S1+((rw_word.action-CLOSE_S1_DRAWER)>>1));
        ps_server_busy = 0;
        break;
    case RESET_S1:
    case RESET_S2:
    case RESET_S3:
    case RESET_S4:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        Do_Action_Reset(S1+(rw_word.action-RESET_S1),rw_word.mp[role].max_speed);
        ps_server_busy = 0;
        break;
    case DISABLE_MOTOR:                             //解锁所有电机
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        Do_Action_Disable();
        ps_server_busy = 0;
        break;
#ifdef PRODUCT_DEBUG
    case S_MOVE_TO:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        if(rw_word.action_arg[0] < MACHINE_COUNT)
            Do_Action_S_Move_To(rw_word.action_arg[0],rw_word.action_arg[1],rw_word.mp[rw_word.action_arg[0]].max_speed);
        ps_server_busy = 0;
        break;
#endif
    case CONFIG_S1_PARAM:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        if (enter_configure_work[0])
        {
            enter_configure_work[0] = 0;
            Config_Param(S1);
        }
        ps_server_busy = 0;
        break;
    case CONFIG_S2_PARAM:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        if (enter_configure_work[1])
        {
            enter_configure_work[1] = 0;
            Config_Param(S2);
        }
        ps_server_busy = 0;
        break;
    case CONFIG_S3_PARAM:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        if (enter_configure_work[2])
        {
            enter_configure_work[2] = 0;
            Config_Param(S3);
        }
        ps_server_busy = 0;
        break;
    case CONFIG_S4_PARAM:
        break;
    default:
        break;
    }
}

void Main_Loop(void)
{
    uchar role;
    Update_WatchDog();
    LED_BLINK();

    Update_IO();

    for(role = S1; role < MACHINE_COUNT;++role)
    {
        if (srd_desc[role].srd->need_cleanup && srd_desc[role].srd->cleanup)
            srd_desc[role].srd->cleanup(role);
    }

    // 一帧消息接收完成
    if (uart0_rx_pending && Uart0_FrameEnded())
    {
        uart0_rx_pending = 0;
        REN0 = 0;
        usart_interrupt_disable(USART0, USART_INT_RBNE);
        Process_Request();
    }
    // 应答结束, 允许接收
    if (!REN0 && !uart0_tx_pending)
    {
        uart0_len = 0;
        uart0_rx_pending = 0;
        REN0 = 1;
        usart_interrupt_enable(USART0, USART_INT_RBNE);
    }
}


void Process_Main_Loop(void)
{
    uchar i;
    Main_Loop();

    for(i = 0; i < KEY_START_COUNT; ++i)
    {
        Process_Starter(i);
        Process_Event(i);
        Key_Led_Ctrl(i);
    }

    if (pending_action)
        longjmp(main_loop_env, 1);
}

void Tst_Systick_Timer6(void)
{
    static uint32_t j[4] = {5,9,13,17};
    static uint32_t i[4] = {2,4,6,8};
    static uint32_t t = 2;
    static ulong stamp = TIMER_STOPED;
    if(stamp == TIMER_STOPED )
        TIMER_START(stamp);
    if((stamp != TIMER_STOPED && TIMER_OUT(stamp,100000)))
    {
        t++;
        t%=4;
        TIMER_STOP(stamp);
        i[t] = j[t] - i[t];
        pending_action = 1;
        rw_word.action = i[t];
        me_state[t] = ME_STATE_WORKING;
    }
}
